/***************************************************************************************************************************
* global2local.h
*
* Author: SFL
*
* Update Time: 2021.04.28

* Introduction:  用来实现全局坐标（lla）对局部坐标（enu）的转换
            1.用来补充Frame_tf_utils.h的，实现lla向ecef再向enu的转换
            2.用来生成轨迹的关键点
            3.配合某个和数传对接的节点，可以实现将其他无人机的local position发布出来
***************************************************************************************************************************/
#ifndef GLOBAL2LOCAL_H
#define GLOBAL2LOCAL_H

#include <mavros_msgs/HomePosition.h>

#include <GeographicLib/Geocentric.hpp>
#include <geographic_msgs/GeoPointStamped.h>
#include <mavros/mavros_plugin.h>
#include <px4_command_utils.h>

using namespace std;

GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(),
				GeographicLib::Constants::WGS84_f());         //地理坐标系转换
//earth.Forward(real lat, real lon, real h, real& X, real& Y, real& Z)
//earth.Reverse(real X, real Y, real Z, real& lat, real& lon, real& h)

// 函数：由经纬海拔坐标转换为局部东北天坐标
// 输入：原点的经纬海拔，当前点（要计算位置）的经纬海拔
// 输出：当前点（要计算位置）在原点建立的东北天坐标系中的坐标
Eigen::Vector3d lla_to_enu(Eigen::Vector3d origin_lla, Eigen::Vector3d cur_lla)
{
    Eigen::Vector3d local_ecef,vec,origin_ecef,local_enu;
    double X;
    double Y;
    double Z;
    double latitude = cur_lla[0];
    double longitude = cur_lla[1];
    double altitude = cur_lla[2];
    earth.Forward(latitude, longitude, altitude, X, Y, Z);
    local_ecef[0] = X;
    local_ecef[1] = Y;
    local_ecef[2] = Z;

    latitude = origin_lla[0];
    longitude = origin_lla[1];
    altitude = origin_lla[2];
    earth.Forward(latitude, longitude, altitude, X, Y, Z);
    origin_ecef[0] = X;
    origin_ecef[1] = Y;
    origin_ecef[2] = Z;
    vec = local_ecef - origin_ecef;
    local_enu = mavros::ftf::transform_frame_ecef_enu(vec, origin_lla);
    local_enu[2] = cur_lla[2];
    // cout << local_enu << endl;
    return local_enu;
}

// 生成三次样条规划的关键路径点
// 输入：原点lla，框的位置lla，框的穿越角度（弧度）在东北天坐标系下的，这里指的是飞机yaw为这个角时正好能识别框并且穿越过去
// 输出：enu坐标系下的x和y
// 因为今年有两个框，这个函数需要调用两次，分别代入框1的位置和航向与框2的位置和航向。
// 如果有再多的框的话就按照穿越顺序调用本函数即可
void create_local_path_key_point1(Eigen::Vector3d origin_lla, Eigen::Vector3d box_lla, float box_cross_heading, vector<double>& x, vector<double>& y)
{
    Eigen::Vector3d box_enu = lla_to_enu(origin_lla, box_lla);
    float body_frame[2] = {-500.0, 0.0};
    float enu_frame[2];
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -450.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -400.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -350.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -300.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -200.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -100.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 0.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 100.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 200.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 250.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    // cout << box_enu << endl;
}

void create_local_path_key_point2(Eigen::Vector3d origin_lla, Eigen::Vector3d box_lla, float box_cross_heading, vector<double>& x, vector<double>& y)
{
    Eigen::Vector3d box_enu = lla_to_enu(origin_lla, box_lla);
    float body_frame[2] = {-500.0, 0.0};
    float enu_frame[2];
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -450.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -400.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -350.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -300.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -200.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = -100.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 0.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 100.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 200.0;
    body_frame[1] = -10.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 250.0;
    body_frame[1] = -20.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    body_frame[0] = 300.0;
    body_frame[1] = -30.0;
    px4_command_utils::rotation_yaw(box_cross_heading, body_frame, enu_frame);
    x.push_back(box_enu[0] + enu_frame[0]);
    y.push_back(box_enu[1] + enu_frame[1]);
    // cout << box_enu << endl;
}


#endif //GLOBAL2LOCAL_H
